/*
 * @Author: Meroke
 * @Date: 2022-04-02 13:48:20
 * @LastEditors: Meroke
 * @LastEditTime: 2022-04-07 20:18:58
 * @Description: file content
 * @FilePath: \MDK-ARMe:\Program\Code\cpp\STM32\project\arm_test\Servo\Src\servo_mode.c
 */
#include "servo_mode.h"


// Link function pointers with function
uint8_t Register_Init(Servo_Control *sc)
{
  sc->_servo_init=servo_init;
  sc->_servo_move = servo_move;
  sc->_servo_set_angle = servo_set_angle;
  return 1;
}

// servo attribute init
uint8_t servo_init(Servo_Control *sc,TIM_HandleTypeDef * htim,uint32_t channel_group[],uint8_t angle[],uint8_t servo_len)
{
    sc->_htim = htim;
    sc->_channel_group = channel_group;
    sc->_current_angle = angle;
    sc->_target_angle = angle;
    sc->_servo_len = servo_len;
    sc->_stop_flag = STOP;
    return 1;
}

// target_angle' setter
uint8_t servo_set_angle(Servo_Control *sc,uint8_t angle[])
{
    sc->_target_angle = angle;
    return 1;
}

// stand alone - driven servos
uint8_t servo_move(Servo_Control *sc)
{
  for(uint8_t i=0;i< sc->_servo_len;i++)
  {
    uint8_t cur_angle = sc->_current_angle[i];
    uint8_t tar_angle = sc->_target_angle[i];
    if(tar_angle > 180 || cur_angle == tar_angle)
      continue;
    if(cur_angle < tar_angle)
    {
      for(;cur_angle < tar_angle;cur_angle ++)
      {
        PWM_driving_servo(sc->_htim,sc->_channel_group[i],cur_angle);
        HAL_Delay(3);
      }
    }
    else
    {
      for(;cur_angle > tar_angle;cur_angle --)
      {
        PWM_driving_servo(sc->_htim,sc->_channel_group[i],cur_angle);
        HAL_Delay(3);
      }
    }
    sc->_current_angle[i] = tar_angle;
  }
    // HAL_Delay(1000);
  return 1;
}

// Concurrency - driven servos
uint8_t servo_move_group(Servo_Control *sc)
{
  if(sc->_stop_flag==STOP)
    return 1;

  while(sc->_stop_flag == RUN)
  {
    for(uint8_t i = 0; i< sc->_servo_len;i++)
    {
      if(sc->_current_angle[i] == sc->_target_angle[i])
        sc->_stop_flag = STOP;
      else  
      {
        sc->_stop_flag = RUN;
        break;
      }
    }
    for(uint8_t i = 0; i< sc->_servo_len;i++)
    {
      if(sc->_current_angle[i] != sc->_target_angle[i])
      {
        uint8_t cur_angle = sc->_current_angle[i] + (sc->_target_angle[i] > sc->_current_angle[i] ? 1:-1);
        PWM_driving_servo(sc->_htim,sc->_channel_group[i],cur_angle);
        sc->_current_angle[i] = cur_angle;
      }
      HAL_Delay(5*sc->_servo_len);

    }
  }
 
  return 1;
}

// !! Base method of driving servo 
void PWM_driving_servo(TIM_HandleTypeDef * htim,uint32_t Channel,uint8_t angle)
{
	uint16_t compare_value=0;
	if(angle<=180)
	{
		compare_value=(0.5+angle/90.0)/20.0*20000 -1.0;
		__HAL_TIM_SET_COMPARE(htim, Channel, (uint16_t)compare_value);
	}
}

// In absolute angle forces a move the servo
uint8_t force_move_angle(Servo_Control *sc,uint8_t target_angle)
{
  PWM_driving_servo(sc->_htim,sc->_channel_group[1],target_angle);
  HAL_Delay(1500);
  return 1;
}


/*
  Action Mode
*/

//Temp test painter mode
uint8_t grab(Servo_Control *sc)
{
  uint8_t angle[3]={180,60,0};
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(1000);

  angle[0] = 180;
  angle[1] = 0;
  angle[2] = 0;
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(500);
  angle[0] = 180;
  angle[1] = 0;
  angle[2] = 30;
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(500);

  angle[0] = 0;
  angle[1] = 0;
  angle[2] = 0;
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(1000);
  return 1;
}
uint8_t swing(Servo_Control *sc)
{
  uint8_t angle[3]={60,30,0};
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(1000);
  angle[0] = 180;
  angle[1] = 30;
  angle[2] = 30;
  sc->_stop_flag = RUN;
  sc->_servo_set_angle(sc,angle);
  sc->_servo_move(sc);
  HAL_Delay(1000);
  return 1;
}
uint8_t homing(Servo_Control *sc)
{
  for(uint8_t i=0;i<sc->_servo_len;i++)
  {
    PWM_driving_servo(sc->_htim,sc->_channel_group[i],0);
  }

  sc->_current_angle[0]=0;
  sc->_current_angle[1]=0;
  sc->_current_angle[1]=0;
  HAL_Delay(1000*sc->_servo_len);
  return 1;
} 

uint8_t select_mode(Servo_Control *sc, uint8_t mode)
{
  switch(mode)
    {
      case HOMING:
      {
        sc->_select_mode = homing;
        sc->_select_mode(sc);
        HAL_Delay(1000);
        break;
      }
      case GRAB:
      {
        sc->_select_mode = grab;
        sc->_select_mode(sc);
        HAL_Delay(1000);
        break;
      }
      case SWING:
      {
        sc->_select_mode = swing;
        sc->_select_mode(sc);
        HAL_Delay(1000);
        break;
      }
      default:
        break;
    }
  return 1;
}
